// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*!
 * @file SensorCombinedPublisher.cpp
 * This file contains the implementation of the publisher functions.
 *
 * This file was generated by the tool fastcdrgen.
 */


#include <unistd.h>         //Used for UART
#include <fcntl.h>          //Used for UART
#include <termios.h>        //Used for UART

#include <fastcdr/Cdr.h>
#include <fastcdr/FastCdr.h>
#include <fastcdr/exceptions/Exception.h>

#include <fastrtps/participant/Participant.h>
#include <fastrtps/attributes/ParticipantAttributes.h>
#include <fastrtps/publisher/Publisher.h>
#include <fastrtps/attributes/PublisherAttributes.h>

#include <fastrtps/Domain.h>

#include <fastrtps/utils/eClock.h>

#include "SensorCombinedPublisher.h"




SensorCombinedPublisher::SensorCombinedPublisher(): mp_participant(nullptr),
                                                    mp_publisher(nullptr),
                                                    m_uart_filestream(0),
                                                    m_uart("")
{}

SensorCombinedPublisher::~SensorCombinedPublisher() {	Domain::removeParticipant(mp_participant);}

uint8_t SensorCombinedPublisher::init_uart()
{

    //OPEN THE UART
    //The flags (defined in fcntl.h):
    //  Access modes (use 1 of these):
    //      O_RDONLY - Open for reading only.
    //      O_RDWR - Open for reading and writing.
    //      O_WRONLY - Open for writing only.
    //
    //  O_NDELAY / O_NONBLOCK (same function) - Enables nonblocking mode. When set read requests on the file can return immediately with a failure status
    //                                          if there is no input immediately available (instead of blocking). Likewise, write requests can also return
    //                                          immediately with a failure status if the output can't be written immediately.
    //
    //  O_NOCTTY - When set and path identifies a terminal device, open() shall not cause the terminal device to become the controlling terminal for the process.
    m_uart_filestream = open(m_uart.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);      //Open in non blocking read/write mode
    if (m_uart_filestream == -1)
    {
        //ERROR - CAN'T OPEN SERIAL PORT
        printf("Error - Unable to open UART '%s'.  Ensure it is not in use by another application\n", m_uart.c_str());
    }

    //CONFIGURE THE UART
    //The flags (defined in /usr/include/termios.h - see http://pubs.opengroup.org/onlinepubs/007908799/xsh/termios.h.html):
    //  Baud rate:- B1200, B2400, B4800, B9600, B19200, B38400, B57600, B115200, B230400, B460800, B500000, B576000, B921600, B1000000, B1152000, B1500000, B2000000, B2500000, B3000000, B3500000, B4000000
    //  CSIZE:- CS5, CS6, CS7, CS8
    //  CLOCAL - Ignore modem status lines
    //  CREAD - Enable receiver
    //  IGNPAR = Ignore characters with parity errors
    //  ICRNL - Map CR to NL on input (Use for ASCII comms where you want to auto correct end of line characters - don't use for bianry comms!)
    //  PARENB - Parity enable
    //  PARODD - Odd parity (else even)
    struct termios options;
    tcgetattr(m_uart_filestream, &options);
    options.c_cflag = B115200 | CS8 | CLOCAL | CREAD;     //<Set baud rate
    options.c_iflag = IGNPAR;
    options.c_oflag = 0;
    options.c_lflag = 0;
    tcflush(m_uart_filestream, TCIFLUSH);
    tcsetattr(m_uart_filestream, TCSANOW, &options);

    return 0;
}

bool SensorCombinedPublisher::init(std::string &uart)
{
	// Create RTPSParticipant

	ParticipantAttributes PParam;
	PParam.rtps.builtin.domainId = 0;
	PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
	PParam.rtps.setName("Participant_publisher");  //You can put here the name you want
	mp_participant = Domain::createParticipant(PParam);
	if(mp_participant == nullptr)
		return false;

	//Register the type

	Domain::registerType(mp_participant,(TopicDataType*) &myType);

	// Create Publisher

	PublisherAttributes Wparam;
	Wparam.topic.topicKind = NO_KEY;
	Wparam.topic.topicDataType = myType.getName();  //This type MUST be registered
	Wparam.topic.topicName = "SensorCombinedPubSubTopic";
	mp_publisher = Domain::createPublisher(mp_participant,Wparam,(PublisherListener*)&m_listener);
	if(mp_publisher == nullptr)
		return false;
	cout << "Publisher created, waiting for Subscribers." << endl;

	m_uart = uart;
	init_uart();

	return true;
}

void SensorCombinedPublisher::PubListener::onPublicationMatched(Publisher* pub,MatchingInfo& info)
{
	if (info.status == MATCHED_MATCHING)
	{
		n_matched++;
		cout << "Publisher matched" << endl;
	}
	else
	{
		n_matched--;
		cout << "Publisher unmatched" << endl;
	}
}

uint8_t SensorCombinedPublisher::readFromUART(SensorCombined &st)
{
//----- CHECK FOR ANY RX BYTES -----
    if (m_uart_filestream != -1)
    {
        // Read up to 255 characters from the port if they are there
        char rx_buffer[1014];
        int rx_length = read(m_uart_filestream, (void*)rx_buffer, sizeof(rx_buffer)); //Filestream, buffer to store in, number of bytes to read (max)
        if (rx_length != 72)
        {
            printf(".");
            return 1;
        }
        else
        {
            //Bytes received
            eprosima::fastcdr::FastBuffer cdrbuffer(rx_buffer, sizeof(rx_buffer));
            eprosima::fastcdr::Cdr cdr_des(cdrbuffer);
            cdr_des >> st.timestamp();
            cdr_des >> st.gyro_rad();
            cdr_des >> st.gyro_integral_dt();
            cdr_des >> st.accelerometer_timestamp_relative();
            cdr_des >> st.accelerometer_m_s2()[0];
            cdr_des >> st.accelerometer_m_s2()[1];
            cdr_des >> st.accelerometer_m_s2()[2];
            cdr_des >> st.accelerometer_integral_dt();
            cdr_des >> st.magnetometer_timestamp_relative();
            cdr_des >> st.magnetometer_ga()[0];
            cdr_des >> st.magnetometer_ga()[1];
            cdr_des >> st.magnetometer_ga()[2];
            cdr_des >> st.baro_timestamp_relative();
            cdr_des >> st.baro_alt_meter();
            cdr_des >> st.baro_temp_celcius();
            printf("read %d bytes ", rx_length);
            printf("accelerometer: %04f %04f %04f\n",
                    st.accelerometer_m_s2()[0],
                    st.accelerometer_m_s2()[1],
                    st.accelerometer_m_s2()[2]);
            return 0;
        }
    }

    return -1;
}

void SensorCombinedPublisher::run()
{
	while(m_listener.n_matched == 0)
	{
		eClock::my_sleep(250); // Sleep 250 ms
	}

	// Publication code
	SensorCombined st;

	/* Initialize your structure here */

	int msgsent = 0;
	do
	{
		if(0 == readFromUART(st))
		{
			readFromUART(st);
			mp_publisher->write(&st);  ++msgsent;
			cout << "Sending sample, count=" << msgsent << endl;
		}
		usleep(100000);

	}while(true);
}
